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Invention Title: 

Movement Sensing and Monitoring System 
rhe invention is described in the following statement: 



Movement Sensing and Monitoring System 
Field of th e Invention . 

5 mov~ f taVal K° n relateS 10 * SyStem " d me,h0d for »»»■*• -* ae 
for amamg and monitoring foe movement of tan™ fa conjunction with fte^e of 

also relates to a method and apparatus to enable stimulation of muscles of a subject iri 
^-^sTOHttatmeaubjectperformsanaoavity: c es ot a subject m 

Backemiin^ to the Tny «p« m 

mn „ W " ° S rf 6 " 1 ' ^'k^ 0 " No 2001/OOOH25A1 there is disclosed a aystem for 

Thesear^ f, "** "^"'^ micro-aensors and micm-taXd^ 

20 ZrdtaT ! "e Programmed to transmit commands to the micro-srimulators 

Zn^tode, 5 ,rea ' ment ^ M T m ~ "elegit 

parameters to detenmne apabenfs stems or me effecriveneaa of a treatment rngta 1 

to euan^T ", « * ^ *" ilrfoma «-» « *» movement of an object 

£ reS,K>nSe &r PUIP0M ° f - oontmlling subsJem 

25 vetaXTf b A Parta#ar ° DjeCL ^ Whae *« — J- S 

~VZ£r ,8 , me T red ta ^ *" " " Preferabk to *" « « 
LTZIT """^ thn>U8h m *«*. to perform various to* such 

aa^tong, stondmg or utaaung a step for foe object, and in parucuta a tamar, aa a 

r^h^foe meaauremen. of acceleradon and angular velocity. Alternative^ such 
30 ZnT ° f a0Celera,,0n ^ **** ^ »° *r turfoer diagn Js and 

time T**" d0OUme,,, *" disolose » «»' Performs real 

bme momtonng and measuring of foe various pammeters and more parLarly does 

ealcmatmgrealumepoaittonandvelocityoffoemovingobject 

awav J*? N ° 5 ' 919 ' 149 «osea a aystem for monitoring postural 

sway of a human aubject during standing or movemen, tasks. This ia an aid in foe 
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rehabU.tat.on of balance and gait deficiency and more particularly provides feedback 
of postural m&nnation to the subject such as in a visual format by way of an imaging 
system mounted on a pair of eyewear used by the subject or in auditory fonn or tacnie 
torn, or alternatively in the torn, of elector stimulation to the vestibular nerve 
5 ft. European Patent No 799597 there is disclosed an invention relating to a 

prosmehc apparahra mat augments balance signals normally used by a human brain to 
centre me body in a stable position and prevent a fall. It also provides jnfonnation to a 
subject on .ts state of balance. More particularly though it relates to a balance 
prosfc es.s apparatus mat measures angular velocity and position of a hunk of asubjeet 
10 u, three orthogonal axes and provides information on me changes in the amplitude and 
t^TTr! an 8 ul *» 1OT ^ over .ume to me. subject using aimer vibro- 

tactile, audnory, v,sual or direct electrical stimulation to the subject 

me., "TuT" Pate " N ° 6>063,046 diTCl0Ses anKth ° d — Warams mat 

15 reSP0DSe ° f 3 PUOed ta - ' **** on a support 

15 surface winch .s moved in any combination of pitoh and roll directions. The 

measurements are made using force transducers, body sway sensors such as velocity 
transducers «| EMG electrodes mounted over muscles on the left and right side of the 
subject s body. The measurements are displayed to an operator together with the 
response measurements from a normal sample population From the displayed 
20 measure me operator may ^ ^ ^ ^ ^ ^ * 

balance correction abnormality. 

None of the above prior art documents disclose a system and method that 
measures Imear acceleration and angular velocity of a subject to calculate, the real time 
posmon and velocity of the moving subject over time with a view to altering or 
25 adjusfcng sagnals delivered to a prosthetic device in order to affect the movement of 
n^ch* of the subject- to achieve a desired outcome such as walking, sitting or 



30 



Summary of the Tnvftntirm 

According to a first aspect of the invention there is provided apparatus to enable 
stimulation of muscles of a subject in real-time such that the subject performs an 
activity, the apparatus comprising: 

Sensor means for monitoring and measuring movement of the subject; 
Controller means linked to the sensor means for receiving data from the sensor 
means indicative of the muscle activity; and 
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Stimulator means linked to the controller means for stimulating the muscles in 
accordance with commands delivered by the controller means in response to the data 
received from the sensor means. 1 

According to a second aspect of the invention there is provided apparatus for 
5 detecting and recording movement of a portion of a subject in response to stimulation 
of muscles of the subject, the apparatus comprising: 

Stimulator means for stimulating the muscles; 

Sensor means linked to the stimulated muscles for recording data pertaining to 
movement of the portion of the subject; " 

10 , Wher6in 40 recorded data is forwarded to a controller means for analysis in 
real-time and whereupon a determination is made to send a command to the stimulator 
means based on the recorded movement data. 

The portion of the subject may be any one or more of limbs of the subject, the 
^rso of the subject, or muscles for controlling the bladder or seated pressure relief 
15 The sensor means may include one or more accelerometers for measuring three 
dimensional linear acceleration of the portion and angular velocity measurements 
means, such as a gyroscope, for measuring the angular velocity of the portion The 
acceleration and angular velocity of the portion is measured instantaneously and 
transmitted to the controller means. 
20 The stimulator means may be in the form of a series of electrodes. The 

stimulator means may be a surface stimulator which is placed on the surfaceof the skin 
of the subject over the muscle to be stimulated. 

Such a stimulator means may include electrode pads. The. apparatus may be 
used to implement functional upright mobility, bladder control, seated pressure relief 
and lower extremity exercise of the subject. The sensor means may include any one of 
a torso sensor, a shank sensor, preferably one for each leg in the floor reaction 
orthosis(FRO) of the subject, a thigh sensor preferably one for each leg, a strain gauge 
a posterior pressure sensor or an. anterior pressure sensor, all preferably one for each 
leg. Preferably the pressure sensors are linked to respective shank sensors , and 
preferably the shank sensor on each leg is connected respectively to each thigh sensor. 

The controller means may be in the form of a body worn controller, worn 
externally on the subject, linked to each sensor means via a SPI bus. 

According to a third aspect of the invention there is provided sensor apparatus 
for detecting movement of a portion of a subject comprising: 
35 microcontroller means; 



25 
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H^,T ler0meter mam! ^ me "™ iX,g m ° Veme,lt " * m0st ^ons 
linked to the microcontroller means; and 

angular velocity measurement means also linked to the microcontroller means 
tor measuring angular velocity in one axis; 

5 wherein the microcontroller means receives datasignals from the accelerometer 

means and the angular velocity measurement means and after processing, forwards the 
data signals to a controller means for further analysis. 

of Ae accelerometer means and the angular velocity measurement means 
may be linked to the microcontroller means through signal conditioning means . the 
10 sensor apparatus may further receive signals from a strain gauge and- anterior and, 
posterior pressure sensors. Preferably the signals are received by me microcontroller 
means through further signal conditioning means. 

The sensor apparatus is preferably linked to the controller means through the 
microcontroller means via an SPI bus. 
15 The angular velocity measurement means may be in the form of a gyroscope for 

measuring angular velocity of the portion of the subject in one axis of rotation and for 
providing angular velocity feedback to the microcontroller means. 

The microcontroller means may have an analog to digital converter for 
converting analog data received from the signal conditioning means into digital data. 
The signal <™dmoning means that receives the output.from the gyroscope may scale 
the output of the gyroscope so that the signal fills the available range of theanalog to 
digital converter. ~ & 

The accelerometer means preferably comprises two accelerometers, each of 
which measures acceleration in two of three dimensions but combine to measure all 
flrree dimensions The signal from the strain gauge, which is preferably attached to the 
FRO, is preferably representative of the degree to which the subject leans into the PRO 

/-ypcT ?°. T S ° r ^ C ° mprise a I**-** *terfece, Preferably 

QPSI physical interface, that enables the controller means to send and receive , data 
from the sensor apparatus, enable power transmission and programming from the 
controller means. 

According to a fourth aspect of the invention there is provided controller 
apparatus for receiving data from sensor means that monitors and measures movement 
of a portion of a subject, the controller apparatus comprising: 

a sensor interface means for sending commands to the sensor means and • 
Jo receiving data from the sensor means; and 
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tamsceiver means for sending ^ ,pd oommands to an implant device and 
for receiving telemetry data from the implant device: oey.ce.and 

subject. 111 ' ^ mKfflS ^ 66 ° T m ° re SenS0 ' 5 attachable to P 0 "*™ °f 

5 •The controller apparatus may further comprise data storage means for storin. 

d^mcMmg means for identifying a sensor or multiple sensors The apparency 

X ml!n!!; m T da ' a »«« and from each sensor. The 

.0 a^Trs " be m «— *» «* a software module in me one or more 

sensor ^JT'' 0 ^ **** °° mpHS ° * »«W •» *• 

-nsor mtert.ce means for connecting, activating and/ OT deactivating me sensor means 

and for readrng subject portion orientation from the sensor means ,' 
15 ^IpuWicnmct.ons,nonAPIpublicfunctionsorprivatefuncHons 

2r^ST- " - ' — — rpregre*. 

receiving analog values from one or more measurement devices- 
converting the received analog values into digital values; and 
storing the current digital values in memory means- 

mem n J h6rein °°f 0Uer mCanS *"* aCC6SS 3 Spedfic CUrrent ^« ^ue from the 
memory means without waiting for a new analog to digital converted value 

^ According to a sixth aspect of the invention there is provided a method of 
25 ™catmg between a controller means and a sensor mean, using program meajf 
such that command data is sent from the controller means to the seL means th 
method comprising the steps of: 

undertT ° f COmmand ^ by sensor ™**> *e Program means 

undertaking the command according to the command data- and 

comma ^ Smitting a reSp0nSe to ^ller means as a result of undertaking the 

The command data may include any one or more of the following- 

• devicJt 7??** ValueS from analog values from measurement 

devices, the digital values being stored in memory means; 

35 b) turning off measurement devices in the sensor means or external to the 

sensor means; 
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coronal plane. ' * *" ^ maas to tte Plane or 

caJZtZZ HT* ° f ^ iDVe, " i0n *" iS * ™»- of 

d^nga.mcd.anofouva.uesforeachmeawemen.obtamed; 
storing each determined median; and 

15 rating A. above stepa for each subsequent position of the sensormeana 

P^fe^edrnintenn.gr.vitil^^, 8 „! " » » 

20 average of these two ™ ™' "° fa *» - h —" - *« 

(gyroscope) 0. ""S" 1 " v *"*y measurement 

OSPI conunJT meanS may tra,,smit """""d data via a 

QSPI communtcabona p ro toco. to the sensor.means to read the values stored iTm! 
memory means ^ towntetothe memory means calihmnon constant 

subject, the protocol comprising: p ni0noU 

for sendlTto° n i ayer f<>r PTOCeSS ^ Md fo — data from an application layer and 
for sending to and retnevingdata from a memory means- 

35 between^ ^ ^ t0 SeSSi ° n ^ for recei ™* and transmitting data 

■>:> ^tween the session layer and a physical layer; and 
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means^f^ 031 ^ ^ ^^ons between the: controller 

means and the sensor means. ■ 

The physical layer may also communicate with transmit and receive buflera 

enabling 8 8 ^ *"* "* *° merc is P™vided a method of 

enabltng eommnmeanon between a sensor means and a oontroUer means using a 
program, the method comprising the steps of: 

using an application layer assoeiated with the sensor means for receiving and 
processmg commands from to confroller means, performing the required cZ^ 
and responding to the controller means. recpnrea command 

task in^rf 0 " """^ «" S,epS of '"»-*« = hackgmumi 

task m the sensor means, when no other command process is being canied ojby me 

SLIT aaBsto8 ae — ' m ™ md — - — 

■ink .av^^wT *• *** of Processing data to and from a data 

■ink layer and to and from a physical layer, through a receive buffer. 

Brief Degi-ri ption 0 f ,h f rv.^ 

Preferred embodiments of the invention will hereinafter be described by way of 
example only, with reference <o the drawings wherein- * 

JXLs" 3 W<>Ck ° f °™ *° according to 

lens Jl" ^ ° f * hUman — " to ■»«»> and, 

legs together with a controller linked to each of the sensors- 

Figure 3b is a block diagram of one of the sensors used in the invention- 
Figure 4 is a circuit diagram of a 3:3 volt power supply- 
Figure 5 is a circuit diagram of a 5 volt power supply; 
Figure 6 is a circuit diagram of a 1.24 volt reference- ' 
^ Fi ^ e7is ^uitdiag^^ . 

to be ZZ1 " ^ dia8ram ° fan aCCeIer ° meter * at ^les a,elf test fuhction 
to oe perrormed on the sensor; 

Figure 9 is a circuit diagram of a signal conditioning unit- 
Figure 10 is a circuit diagram of a strain gauge signal condoning unit; 
Figure 1 1 is a circuit diagram of a microcontroller of a sensor 
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Figure 1 la is a circuit diagram of the QSPI and programming interface; 
Figure lib is a circuit diagram of the microcontroller interface circuit that 
allows programming of the sensor; 

Figure 12 is a circuit diagram for converting a measured strain into a voltage for 
on forwarding to a microcontroller; 

Figure 13 is a circuit diagram of a QSPI physical interface; 
Figure 14 is a circuit diagram 6f a circuit that enables the controller to read 
analog inputs via a sensor; 

Figure 15 is a block diagram showing the architecture of the controller; 
Figure 16 is a block diagram showing the connection of various sensors to the 
controller through a QSPI bus; 

Figure 17 is a block diagram showing implementation of a sensor firmware 
including various protocol stack layers; 

Figure 18 shows six different orientations that the sensors are placed in in order 
15 to calculate the certain values; 

Figure 19 depicts angle measurement by one of the sensors; 
Figure 20 shows a system for measuring the acceleration vector due to gravity 
using accelerometers; 

Figure 21 is a diagram showing the architecture and implementation of a $SPI 
20 communications protocol; 

Figure 22 shows a block diagram of protocol layers on the controller side and 
QSPI device side and the data flow therebetween; 

Figure 23 is a block diagram illustrating various messages between each of the 
layers of the master controller and slave devices; 

25 Figure 24 depicts a state diagram showing the general format of commands sent 

to a QSPI device; 

Figure 25 is a state diagram regarding the QSPI identification command; 
Figure 26 is a state diagram relating the session layer in a slave QSPI device; 
Figure 27 is a state diagram describing the data link layer in a slave QSPI 
30 device; and 

Figure 28 is a state diagram describing the operation of the data link layer in.the 
controller. 



35 



Detailed Description nf th e Prefenred Kmbodiments 

The present invention is comprised of many components all of which have 
varying roles in providing subjects, such as humans, with renewed control over 
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muscles. This is particularly applicable to paraplegics where as a result of spinal chord 
injury they are no longer able to voluntarily control their muscles. In this particular 
embodiment benefits provided by the invention include functional upright mobility 
bladder control, seated pressure relief and lower extremity exercise. 
5 The various modes of operation available include a functional upright mobility 

mode which provides the patient with the ability to initiate movement of the body or to 
maintain stable body position; Specifically, electrodes provide stimulation in- a co- 
ordinated manner to implement the standing function , the step function once the 
patient is standing and the sit function also When the patient is standing. For all 1 three - 
10 modes closed loop control is achieved using various multiple sensor devices. A further 
mode is bladder control mode mat provides the patient with the ability to initiate 
activation of the detrusor or sphincter muscles for the purpose of bladder control 
Electrodes stimulate the patient's sacral roots to achieve bladder control. Another 
mode is the seated pressure relief mode which provides the patient with the ability to 
15 initiate activation of the gluteal muscles to adequately shift body weight and relieve 
seated pressure. Again the electrodes stimulate the gluteal and other muscles 
periodically to shift the patient's weight while sitting. A warning signal with delay 
shall be provided to the patient prior to initiating stimulation. A final mode is the lower 
extremity exercise mode which provides a patient with the ability to initiate movement 
20 of the lower extremities so as to increase muscle strength, decrease fatigue ability and 
increase muscle bulk. The muscles in the lower extremities shall be activated via 
electrode stimulation so as to produce an exercise effect, such as knee extension. 

Shown in Figure 1 is a block diagram of the system with its various components 
that enables the monitoring and measurement in real time of the various parameters 
25 associated with the movement of a patient having an implant device. External to the 
patient there is a navigator controller 10, otherwise known as a Body Worn Controller 
(BWC) that drives the various sensor packs 12 and has a two way communication link 
with each of the sensor packs 12 to record the various measurements and also to enable 
data to be transmitted to a stimulator in response to the various measurements. The 
30 navigator controller 10 controls the operation of a surface stimulator 14 by sending to it 
commands over the interconnecting communication link which is a QSPI link. The 
surface stimulator 14 is a Body Worn Unit which includes a controller printed circuit 
board mounted .in a housing with connections to the controller 10. The stimulation of 
the muscles is performed via electrode pads placed on the surface of the skin over the 
35 muscle. The sensor link or QSPI (Queued Serial Peripheral Interface) link provides 
power and communications between the sensor pack modules 12 and the navigator 
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controller 10 and more specifically provides power- and control from the navigator 
controller 10 to the sensor modules and stimulators) and data from the sensor modules 
andstimulator(s) to the navigator controller 10. The sensors that make up the sensor 
pack 12 comprise body mounted sensor packs with connecting cables and connectors 
5 They provide closed loop control in the upright mobility and- exercise modes. In 
essence closed loop control means that commands transmitted by the navigator 
controller 10 are dependent, in part on the content of the data signals received by the 
navigator controller 10 as a result of feedback. The feedback from the sensors ensures 
safe stand up, Slt down, standing and stepping functionality. They are self-calibrating 
10 and allow for drift in the sensors and movement of .the skin. Power and control 
commands are sent from the navigator controller 10 to the implant device 18 via an RF 
link which includes a RF transmitting coil 16, ? transcutaneous RF link that transmits 
flie power and data to the implant across the skin ofthe patient which in turn is received 
by the implant device 18. The device 18 provides up to 22 channels of stimulation to a 
15 se of electrodes 20 that are used to transmit the stimulation from the implant to 
selected muscle groups. They are also used to transmit telemetry data back to the 
implant and in turn back to the navigator controller unit 10 over the RF link 

The implant 18 is a FES24-B which is similar in concept to that of standard and 
mini cochlear implants. It therefore has some common primary features with the 
20 cochlear nnplants, such as the RF antenna coil, coupling magnet and the receiver- 
stimulator and also has 22 electrodes connected with the extension leads and plug-in 
connectors together with bifurcated electrodes. Similar telemetry features available^ 
the cochlear implants are also available on this particular implant 18. A remote control 
• unit 22 enables patients to fully control live mode operation without necessitating direct 
25 interaction with the controller 10. Thus there will be a wireless communications link 
between the remote control.unit 22 and the navigator controller 10. 

Shown in Figure 2 is a profile of a human having various sensors attached to the 
torso and legs together with the controller 10 (Body Worn Controller). The sensors 
provide instantaneous three dimensional linear acceleration and angular velocity 
30 measurements which are fed back to the controller 10. These signals are then used to 
calculate the instantaneous position and velocity ofthe lower extremities ofthe human 
together with movement of the torso to enable control of the lower limbs during 
standing up, standing, sitting down and stepping operations. The system is designed to 
provide closed loop stable control ofthe lower limbs particularly ofa paraplegic that is 
35 fitted with an electrical stimulation system such as the FES24. 
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for J UenTfl, r ,S SP60ifiCalIy 8h °™ 3 ^ 30 > * —or 32 on, 
for each leg m the floor reaction orthosis (FRO) 36, a thigh sensor 34 again one for 

sensor 42. Each of the sensors are linked to the controller 10 with both L n , 
5 sensors 40 and 42 (on each foot) Hnked to a respective shank lZn t^TT 
ti^gh sensors 34 .and shank sensors 32 are stiapped to 

^2^* ™ e *-» 38 along with the two press™ sens2 £ 
10 32 ? r ° reaCti0n 0rth0Sis 36 and *** «>™°** "> »• ^ sense 

^accelerators and one rate gyroscope which gvmscope measures the angute 

A ° ontroller 10 has three sockets, into each of which a cable may be plugged 

Annmberof sensor^acksntayconnn.nti^ewintaecontioUer 10 along L cable to 
M nuance two sensorpacks on each Lg wU.be hnked to one cable, 

ft* 1 can connect with a single socket is refcred to as a channel. Each JZZ££ 

eight puis terminated a, the controller .0 with a modified RJ-45 plug 

— connections ^ the SP. (Sertal Periphenu mterface), powel supp^l 

20 ZllT i tenCh ' ^ nK, ' ti<med *» » «». «h cab>« 

20 connected to the controller 10. Dies 

conn^TriJ 11 K8Ure 3<a> iS " b ' 0Ck diagram ° f ^ gauges 

mtnfm tffo T ^ " * *** SenS ° r,>a0ks 32 ' " deludes a processing 
25 TaH ^ <*• """"-Ml. 46 to which are linked a gyroscope 48 and a 
25 p^ofacoefaometorsJOandSZ. Each of the gyroscope and accelerometers are-linked 

to flie ™*roller 46 ttuough a signa, Zoning unit 54, 55 and 58 respecti^ 

—H/T « m " !ro ^"" roUer ^Snal conditioning units 60, 62 and 64 

^actively. He nncn>connn.ler46 is linked via fire SPI bus 44 to tire contioUer w 
30 Separate power .uppties and vohage references axe also included in theaensorM 

conn JL" "if ^ ^ ° f *" "■*• "» * «** ma. allows analog 
to just the shank sensor 32. 

35 R ,n< f"° Win8 deSCripti0n is ™* refe ™<* to the components of each of the 

severs 30 32 and 34. Each sensor has one or more power supplies for producing 
afferent voltages to specific components within the sensor. Generally the sensors have 
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a power supply voltage of between 3.4V and 5.25V. This is the input voltage range to 
the power supply which in turn produces an output regulated voltage of 3.3V in order 
to power the imcro-controllers 46 and some signal conditioning units. It also produces 
a regulated 5V output supply in order to power the gyroscope 48 and some of the signal 
conditioning units. Finally a 1.24V reference voltage is produced . from me power 
supply also for use in signal conditioning. 

The 3.3V power supply (see Figure 4) also supplies power to the accelerometers 
50, 52. It converts an unregulated voltage above 3.4V into a regulated voltage of 3 3 V 
The 3.3V power supply contains a National Semiconductor linear voltage regulator 
LP2951. The 5V power, supply may provide power not only to the gyroscope 48 but 
also to the micro-controller 46 and accelerometers 50, 52. The supply may contain a 
Linear Technology Micro-power, regulated charge pump DC/DC converter LTC1516 
The 5V power supply is shown in Figure 5. It is noted that the micro-controller pin 
PA0 (PIN51) is connected to the shutdown control of the 5V supply 80. This allows 
the micro-controller 46 to switch off the power supply for the gyroscope unit. 48 and- 
related signal conditioning unit 54. When the shutdown pin on the converter 80 is held 
low then the 5V supply is switched on. The combination of a ceramic capacitor 82 and 
tantalum capacitor 84 in parallel on the 5V output is used to ininimise the switching 
noise. The micro-controller 46 has the ability to shut down the 5V power supply as 
mentioned previously by shutting or switching off power to the gyroscope 48 and 
gyroscope signal conditioning unit 54 which reduces the overall power consumption 

With regard to the 1.24V reference, the circuit is shown in Figure 6. Scaling of 
the output signals of the gyroscope unit 48 and the accelerometers 50 and 52 requires a 
voltage to be subtracted during signal conditioning. This entity is responsible for 
providing a stable 1.24V reference for use in this function. Thus the 1.24V reference is 
supplied to the signal conditioning units 54, 56 and 58 of the gyroscope and 
accelerometers. Again the input is an unregulated input voltage having a range of 3 4 
to 5,25V and the output is a 1.24V signal having enough current capacity to supply the 
signal conditioning units 54, 56 and 58. As an example the 1.24V reference may 
contain a National Semiconductor Voltage Regulator Model No LM385. 

The gyroscope unit 48 measures angular velocity in one axis of rotation and 
provides angular velocity feedback to the micro-controller 46 and in turn to the 
controller 10. The input supply is 5V and the output voltage represents angular 
velocity and a reference voltage which corresponds to zero angular velocity. Examples 
of gyroscopes that may be used are developed by Murata Model Nos ENC-05DD and 
ENC-05EE both of which are vibratory gyroscopes. These particular gyroscopes have 
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a sca>e of output of l.HmV per % with a maximum of ± 90» per second. Tne voltiflm 
^ : » generafly about ± 10 0mV. The gyroscope unit 48 together wifc J^d 
conditioinngjmitSfisahowiiinFigure?. 

With reganj to the signal conditioning unit 54 it haa the putpose of scaling flu, 

1 If™ 8 ? 6 48 S ° "* ^ ™° ** W L£e of fltetZgt 
fl^M converter m the micm-controller 46. The input to the signal conditioning^, 

be seen from Flgnre 7 operatiollal mpMa 9Q ^ a P 

10 ^ ^ 1,16 "* 48 ^ ° ">PUt stemming flx.nt fte 

Zl^r Tr P, " 0ftheSyK>SC<>Pennil48 - ™« reference votage ia geherat 

Z_ll£ a TT 1,16 vol,a6e ou * u ' - »• ™4e -fLSl 

fihered and amphfied by flie operaflonal amplifier 90 yielding voltage The' 1 24V 

! 5 «) aVaa! * le inPUt ^ " *» "V* "™ Pi» (PF0) (PIN 

^iS^T""^ ^^-combinaflonofaW-Z^ 

L f-1T 7 t e TT ^ °- 3HZ ^ * band ?- fflter -"own 
IS n^? ^ CO " d,tlonin « «« * National Semi-Conductor Dual Op Amp 
Model No RC LMC6462. P ' P 

20 dm- ^ ,T" i ° ned preViousl y "» accelemmeters 50 and 52 are two 

20 drmenston^bm combine to measure all mree dimensions of acceleration. Each linl 

^ ~ ~. <™ » f *• — tensions of acceleration with one oTZ 

££HT, ""^ Pe,pendioul » t0 *• *» so tttat Are linear acceleration 
m flte ^tta! p ,a„e may be manured. Thus, one dimension may be measured twice to 
pmv.de a reference and confirmation of me measurement Also me particular direction 

l:rrr r ^ togravi,yoanbedet ~ 

8 0toge °^ M ^ ndto * to tao ^ons of lme« acceleration. It 
mlTclT m0dulated «** oorresponding to me two dimensions of 

hue. aecelemtioa A self tea, facility is also included whemin me self M sign* is 

30 W iT S . ^ °T ° f,he miCr °^ , « roto <* an input to the acoeleroLer 
LI 7* « — A" various outputs are analog X - 

ax.s aecelertmon. amdog Y - axis acceleration,. PWM X - axis acceleration and PWM 
Y - a*s acceleration. Specific suitable aecelerometers that can be used am ADXL 

to L P ° W t'* 8 9Paa&m be nsed by switcl!in 8 *• 

35 1ft! r 10,6 a0CelerOmeter 50 OT «tak« about 2ms to produce a steble^ignal 

afterpowenug up The bandwidth of the fiherefl ^ 0<ltputs X ^ T m<J y „ 

33Hz. TbesensttivrtyoftheanalogoutputsisS^Vperg. The PWM sensor outputs 
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Xour and Y om encode the acceleration in the duty cycle of these digital outputs, where 
the duty cycle is defined as the ratio of Tl to T2 with Tl being the width of the pulse 
and T2 being the overall period of the on and off times in each cycle ; The zero g 
reading is a 50% duty cycleand the sensitivity is on 12.5% per g. 
5 With reference to Figure 8 a self test (ST) may be performed on the sensor by 

connecting the ST pin to a digital output of the microcontroller 46. The Vdd pins 13 
and 14 are connected directly together and the COM pins 4 and 7 are connected directly 
together and these together with pin T2 are grounded. A decoupling capacitor Cod is 
recommended to be connected between pin 14 and pin 4 having a value of 
10 approximately O.luF. The pins 9 and 10 for Your and Xour are connected to digital 
inputs of the microcontroller 46. The bandwidth, of the analog outputs is set by 
capacitors Cx and Cy and calculated by the following formula: 



1 = 5piF 

15 (2n(32k«)xq A ; r) ) 



-3dB : 



The capacitors are approximately 150nF so that the 3dB cut off frequency for 
analog outputs is 33Hz. T2 is set using R set using. the following formula: 

20 T2 « R sct (Q) 

125MCI 

Here is chosen to be 1MQ which yields T2 equal to 8ms. 
The powerup time is given by the following formula: 
25 *" 

T powerup = (1 6QCfilt+ 0.3) ms {C FILT va. uF) 
where its value is 24.3ms for analog outputs. 

30 The signal conditioning units 56 and 58 for each of the accelerometers scale 

(amplify and filter) the analog outputs of the accelerometers so that the available 
resolution of the AID channels of the micro-controller 46 are used. To fulfil this 
requirement there is 128 bits per g minimum resolution. Each analog input derived 
from the accelerometer will be amplified by a fixed factor and then a voltage subtracted 

35 to bias the centre point. An example of such a signal conditioning unit is manufactured 
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by National Semi-Conductor Model No LMC6462 Dual Op-Amp IC. An example ofa 
circuit used for the signal conditioning unit is shown in Figure 9. . 

The strain gauge unit 38 and its corresponding signal conditioning unit 60 is 
5 ^r mFiSUre10 ' TheStrain ^ 38 iS P ie20ele <**> ^d produces an analog 

The signal conditioning unit converts the strain measured by the strain gauge 
into a voltage that is measured by the A/D converters in the micro-controller 46 The 
signal conditioning unit 60 amplifies the voltage signal from the.strain gauge so that a 
measure ,of ^how much a subject is leamng into the FRO can be obtained. Strain gauge 
10 38 maybe an AMP piezoelectric strain gauge with rivets DT1-028K and the strain 

T 8 ^r!T nditi0ning 60 be a National Semiconductor Dual Op-Amp 
LMC6462. The output of the signal conditioning unit is connected to an A-D input of 
the micro-controller 46. 

The micro-controller 46 enables the navigator or controller 10 to receive sensor 
15 readings via the SPI bus 44. It has several functions including allowing serial 
programming from the controller 10, converting analog signals from the sensor signal 
conditionmg units into digital form, supplying power to the accelerometers, controlling 
the self test feature of the accelerometers and reading the digital outputs of the 
accelerometers. It also enables communication with the controller 10. An example of 
20 fixe micro-controller 46 used is the Atmel ATmegal03L flash micro-controller. . This 
has a flash memory, serial programming capability, eight channel 10 bit A/D 
conversion. 

The microcontroller 46 is depicted in Figure 1 1. Pin Numbers 61 down to 54 
. represent pins that provide inputs to the analog to digital converter and are allocated 
25 pins PF0 (ADC0) to PF7 (ADC7). Respectively from pin 61 down to 54 they accept 
inputs from the accelerometer Y-axis signal conditioning unit, the accelerometer X - 
axis signal conditioning unit, the accelerometer Y - axis signal conditioning unit 
perpendicular), the accelerometer X-axis signal conditioning unit (perpendicular), 
the strain gauge signal conditioning unit, the anterior pressure sensor 42, the posterior- 
30 pressure sensor 40 and the gyroscope signal- conditioning unit 54. Thus all of those 
signals are analog inputs. Digital inputs and outputs are passed through pins 51 down 
to 47, pin 35, and pins 6 to 9 inclusive. Pin 51 controls the 5V regulator so that the 
gyroscope 48 and accompanying signal conditioning unit 54 are powered down by 
asserting this HIGH. Pin 50 when asserted HIGH will self test the accelerometer A that 
35 is on the main board, pin 49 supplies power to this accelerometer,. pin 48 when set 
HIGH self tests accelerometer B that is on the perpendicular board, and pin 47 supplies 
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power to accelerometer B. Pin 35 controls programming to the micro-controller 46 
and pins 6 to 9 provide PWM signals to both accelerometers in each of the X and Y 



axis. 



It is also noted that a low pass filter is inserted between the 3.3 V supply and the 
5 A/D supply. The VCC pin is connected to the 3.3V and a 4MHz crystal 130 is 
connected between two pins as is a 32.768kHz watch crystal 132. 

The signal conditioning units 60 and 62 have for their purpose converting 
resistance as measured by strain gauge 38 and pressure sensor 40 into a voltage to the 
input to the micro-conn-oller 46. Shown in Figure 12 is an example of a circuit 
10 whereby a variable resistance measures the change in strain which is converted into a 
voltage and forwarded to the micro-controller 46. Each pressure sensor resistance 
change in the units 40, 42 has an input of 3.3V and an output voltage that represents the 
pressure exerted on the bottom of the FRO. The pressure sensor as an example may be 
a Flexiforce Single Serial Button or SSB-T force sensor. 
15 As previously mentioned there are two accelerometers on each sensor pack. 

• One of the accelerometers is oriented at right angles to the PCB so that all three 
dimensions of acceleration are measured. The accelerometer flat on the PCB shall 
measure acceleration along the PCB and transversely across the PCB. The other 
accelerometer shall be oriented so that one of its axes of measurement is perpendicular 
20 to the plane of the main PCB. The gyroscope has an output which is a scaled voltage 
that corresponds to the angular velocity of the PCB in the sagittal plane of the body that 
the sensor pack is mounted on. . 

A status light emitting diode is provided to indicate status information to a user 
of each of the sensors. The status LED enables status to be displayed to the user by 

25 allowing the micro-controller 46 to control the flashing of the LED. The LED will be 
constantly on if the micro-controller is in a programming mode or in a reset condition 
The LED will flash under the control of the micro-controller in normal operation. 

A QSPI and programming interface circuit allows the sensor 12 to be 
programmed via the same signal lines and connector as the SPI communications are 

30 transmitted on. The interface circuit connects either the SPI communication pins or the 
prograrnming pins of the microcontroller 46 to the connector, depending on the output 
of the reset generation block signals. The interface is implemented using tristate 
buffers, diodes and resistors and the circuit is shown in Figure 11a. The interface 
controls the signals that appear at the QSPI.MISO (that is the master in slave out with 

35 the sensor being the slave) output of the connector. The MOSI (master out slave in) 
mput is connected directly to both the RXD (prograrnming input) and the MOSI pin On 
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10 



15 



the microcontroller 46. When the QSPI_RESET line is low, the QSPI MISO signal 
will be equal to the TXD (programming output) pin of the microcontroller: ' 

When the QSPI_RESET line is high, the microcontroller 46 is not in 
programming mode and communicatiori may occur on the SPI bus. In this state, the 

~* r, eff t cti : eIy remove *• sensor 12 fr ° m *• « 

cZ7^ ° make 4116 Sensor P resent - high impedance , to the 

QSPI_MIS01me. This is shown in the table below. 




The microcontroller 46 is programmed along separate pins to the SPI bus 44 
The controller 10 has its efficient digital outputs to directly drive these pins. Thus a 
series of logic gates and tristate buffers are used to redirect the SPI bus interface to the. 
programming interface pins. The microcontroller digital cutout PCO is used as an 
IGNORE signal. When the pin is high, the SPI bus acts noimally and when the pin is 
low the microcontroller RESET signal is controlled by the chip select PCS from the 
controller 10. The logic table is shown below. 



Inputs 



PCO 
(micro) 



H 



H 



PCS 
(BWC) 



H 



H 



Outputs 



PDI 

(micro) 



MOSI 
(BWC) 



MISO 
(BWC) 



PDO 
(micro) 



MOSI 
(micro) 



MOSI 
(BWC) 



MOSI 
(BWC) 



MOSI 
WC) 



MISO 
(BWC) 



MOSI 
(micro) 



MOSI 
(micro) 



MOSI 
micro) 



RESET 
(micro) 



H 



H 



H 



SS 

(micro) 



H 



H 
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Shown in Figure lib is the microcontroller interface circuit that enables the 
programming of the sensor. Before the microcontroller 46 is programmed the first 
time, the output at PCO will be low. This will set IGNORE signal to low and hence the 
PCS is passed to RESET. 

To program the microcontroller for the first time, PCS is held low to reset the 
microcontroller. PCS is held low while the microcontroller 46 is programmed. When 
programming is completed, PCS is made high. The program in the microcontroller 46 
will set the PCO digital output to high on boot up. The serial communications is now 
connected to the SPI bus 44. 

For reprogramming, once the software has been downloaded, a command is sent 
to the microcontroller 46 along the.bus 44 instructing the microcontroller 46 to set PCO 
to low. The PCS is then set low to reset the microcontroUer while it is -being 
programmed. There is the possibility of software errors making it impossible to 
reprogram the microcontroller 46. This can occur if PCO is always high and cannot be 
made low. In this situation, there are two ways of programming the microcontroller. 
There will be some small jumpers connected to the RESET, PDI and PDO pins which 
can be used to reprogram the microcontroller. If reaching these pins is impossible, due 
to the sensor 12 being encased in Silastic, then pin 6 of J3 (strain gauge at pressure 
sensor connector) may be shorted to a GND pin, such as pin 5 on J3. This will, set 
IGNORE to low and render the microcontroller 46 programmable. PCI is used to 
ensure that if the sensor 12 is not supposed to be communicating then it presents a high 
impedence to the line. 

Shown in Figure 13 is the QSPI physical interface that provides a means where 
the sensor can communicate, be programmed and receive power from the controller 10 . 
through one of two connectors 140, 142. The interface provides a direct link, between 
the power, ground and SPI signals and the connectors. The inputs are protected from 
electrostatic discharge whereby inductors L5 to L9 become resistive, at approximately 
75 ohms at 100MHz. The communication lines are also protected from noise and 
voltages outside the voltage rails. The SPI communication signal inputs at lines 144, 
146 and 148 are filtered with a low pass filter having a 3dB frequency of 1 .6MHz. This 
helps filter high frequency noise from the signal line. Data is transmitted at a bit rate of 
200kB/s. The SPI communication signal output on line 150 also has a low pass filter 
but a bandwidth ten times greater than that on the other output lines. This takes into 
account that several other sensors and other devices may be connected to the QSPI bus. 
In the event of above or below rail voltages being connected to the communication 
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lines, the BAV99 high speed diodes shown at 151 limit the voltage experienced by the 
sensor. J 



A reset generation circuit is connected between the micro-controller 46 and the 
controller 10 for the purpose of allowing programming of the micro-controUer to be 
5 started by the controller 10 when there is software running on the particular .sensor 

^J^rl T *° miCTO - contrbller 46 **> * mode or programming mode when 
the PCS of the controller 10 is set low after the micro-controller 46 sets pin 35 to low 

External inputs are provided on the sensor 32 and the other sensors 30 and 34 
that allow the controller 10 to read the analog inputs via the sensor. The sensor is 
10 forced into programming mode in the event of a software malfunction. The external 
inputs provide a direct connection between the analog inputs on the bottom connector 
of the sensor and the micro-controller 46. This block also provides protection against 
electrostatic discharge, noise and over.voltage conditions. The particular connector 
provides an external line that can force the sensor into a programming mode "in the 
15 event of the above-mentioned software malfunction. The circuit is shown in Figure 14 
which show the external analog inputs 160, 162 and 164 as including a low pass filter 
having a 3dB frequency of 1.6kHz in order to filter high frequency noise from the 
signal line. The external inputs can be pulled HIGH or LOW by the micro-controller 
by setting the signal on pin 27 HIGH or LOW. The micro-controller can also leave 
ZO these outputs at a high impedance to not pull the inputs HIGH or LOW. 

As mentioned previously the sensor pack 32 provides the ability for the 
controller 10 to control or read the sensor via the SPI bus 44. It also allows 
programming via the bus 44, calibration, and turning on and off components in the 
sensor. Furthermore it provides self tests of the accelerometers and reading of its 
25 analog inputs. Each sensor is able to calculate the dynamic orientation of the sensor in 
motion, measure angular velocity in one plane of motion, measure the acceleration 
components in three dimensions, measure the inputs from three general purpose analog 
devices such as from external sensors including.pressure sensors and be able to control 
three digital outputs. 

30 Each sensor enables the controller 10 to read a digital value that corresponds to 

the physical measurement of the following quantities: 

angular velocity in the sagittal plane of the body segment that the sensor pack is 
in contact with; 

linear acceleration experienced in three dimensions in relation to the body 
35 segment that the sensor is attached to; . 

sagittal angle of the body segment that the sensor is attached, to; 
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coronal angle of the body segment that the sensor is attached to; and 
external inputs as a voltage. 

Controller Sensor Interface 
5 Shown in Figure 15 is the overall architecture for the controller 10 which is in 

272 t: r of sensors 32 - r is a master - siave «~ £ £ 

Z^Z 10 S ^ ^ *" 32 ^ ^ At of the 

conquer 10 is a sensor interface 170 which sends commands, to a sensor pack 

io "no 72 ^ r6CeiVeS ^ fr ° m sensor pack 172 - "™ 

10 mterface 170 is m communication with a strategy unit 174, a supervisor 176- a 
commumcahons interface 178, a data storage unit 180 and QSPI driver 182. The driver 
82 is m data communication with a software module 184 in the sensor 32. The driver 

££T a ? S « P0,t Pr<>Vi ' ing 8 ^ ^ — ^rface HO 

provides an application program interface or API to the strategy module 174 and the 

15 „cau P ns interface 178. The QSPI driver functions arched by melted 

170. Al^mmunications to the sensors are handled through the QSPI driver 182 

operatio^T 8 " "° * ^ for each —or 

operation. It provides formation about specific body segment orientation and angular 

20 2 T EaC \ releVant -Sment is represented by an enumerated type, t a 

20 normal session the following operations are performed: 

the sensor interface 170 is initialised by the supervisor 176- 

the strategy module 174 connects and activates the various sensors; and 

connect^ StrateSY T, dUle rCadS b ° dy S6gment ° rientati o n any or all of the 
connected sensors and deactivates the sensors. 

25 « • ♦ I"™ ?* S6nSOr 170 6nableS E Strate ^ module to find the current 

™Z^T: ^ With ° Ut ^ t0 ^ «~ — unication 

details. Each of the strategy modules call functions in the sensor interface 170 so as to 

obtain the current orientation of the body segments as measured by each of the 

. individual sensors. Each of these functions run in the context of the calling task 

As mentioned previously a strategy module identifies specific sensors by an 

enumerated body segment type. Sensor interface 170 interrogates the data storage 

module 180 in order to find out the serial number of the sensor at this specific body 

segment. The serial number is used to address specific sensors via the QSPI driver 182 

to send sensor commands to the specific sensor and receive sensor information from 

that sensor. The sensor interface 170 also maintains a register of a "replaced sensor". 

At imtialisahon, if one (and only one) expected sensor, is missing and another sensor is . • 
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connected but not registered in data storage module 180, then the serial number of the 
unexpected sensor is stored along with the enumerated body segment of the sensor in 
the data storage module 1 80 that it is replacing. 

The controller 10 preferably comprises an off the shelf pocket PC that is fitted 
5 with a custom designed CF+ interface card that has connections to the RF transmit coil 
16 and a peripheral device link to the sensor packs 12 and the stimulator 14 The 
controller 10 uses the off the shelf pocket PC computer for handling controlling tasks 
and has a controller interface for interfacing to the pocket PC via the compact flash port 
of the pocket PC and provides functions as hereinafter described. The controller, also 

10 includes a telemetry receiver means that is capable of receiving telemetry. . data 
ttansmitted by the CIC3 receiver stimulator within the implant 18. It also includes an 
RF transceiver means for transmitting the data to and from the controller 10 and a date 
encoder formatter that converts the stimulus information, that is active and indifferent 
electrode, amplitude, pulse width and interface, gap, into the embedded protocol for 

15 transmission to the implant component. It also includes an RF interface to the remote 
control unit 22. 

With regard to the software used in the controller 10, preferably the software 
under the name "Clinix" runs under the pocket PC's windows CE operating system 
The software is preferably written in C++ and provides an environment for strategies 
20 for sit-stend-sit, stepping, exercise, bladder control and seated pressure relief to operate 
Preferably the software's operation is configurable via a built-in programming system 
ftmcfronality and is designed so as to minimise the risk of unintentional stimulation 
The software will not operate unless the controller interface is inserted into the pocket 

25 Functions that are contained in. the sensor interface 170 are split into three 

different categories, API public functions, non-API public functions and private 
factions. The API public functions are those that will be supported for future versions 
of the sensor interface including future versions of the sensor hardware. Non-API 
pubhc functions are those which are highly dependent on the current iteration of 

30 software and hardware and may not be supported in- future versions of the sensors 
Private functions are those that may only be called from within the sensor interface 
Each of the sensor interface functions are mostly called by strategies however the 
communications interface 178 may call some sensor functions during a programming 
session , to read sensor information and perform any required calibration. The 
supervisor 176 initialises the sensor interface. Each of the various functions are located • 
in the file Senslntfc. with the prototypes in file Senslntfh. 
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The following is a description of the various sub-functions within each of the 
API public functions: 

API Public Functions 

5 .• 

Sensor Init 

This function initialises the global data for sensor interface 170 so that there is 
no replacement device registered. The interface is initialised by the supervisor 176 by 
calling this function at start up. The function returns an enumerated error type 
10 SensorErrorType 

SensorConnectMutiple 

If this function is called by any strategy that requires sensor information it will 
be called when the strategy is started. The function connects all of the sensors, 
15 switches them on and checks them to see that they are working properly. The argument 
for this function is a pointer to an array of expected enumerated body segments for any 
expected sensors. There is also an argument for the number of expected sensors. The 
return type is an enumerated SensorErrorType. 

20 SensorlsConnected 

This function returns TRUE if the sensor is connected and FALSE otherwise. 
The argument is an enumerated body segment of a single sensor and the return type is a 
Boolean value. 

25 SensorActivate 

This function switches the designated sensor on. The function QSPISendPacket 
is used to send the command. If the sensor is turned on then SensorActiviate returns 
SENSOR_OK. If the sensor did not turn on, then SensorActivate returns 
SENSORJERROR. 

The argument of this function is an enumerated body segment of a signal sensor 
and the return type is an enumerated type SensorErrorType. 



30 



SensorDeactivate 

This function switches the sensors off. QSPISendPacket is used to send the 
35 commands and if the sensor is turned.off and there are no communication errors, then 
this function returns SENSOROK. If the command was unsuccessful then the 
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function returns SENSOR_ERROR. The argument for this function is an enumerated 
body segment of a single sensor and the return type is an enumerated type 
SensorErrorType. 

5 SensorVerify 

This function verifies that a single sensor is operating normally. The 
QSPISendPacket is used to determine the status of the sensor and returns 
SENSQR_ERROR if there is an error that affects the functionality of the sensor. An 
enumerated body segment of a single sensor is the argument for this function and the 
1 0 return type is an enumerated type SensorErrorType. 

SensorGetSagittalOrientation 

This function returns the current value of the angle of orientation of the given 
sensor in the sagittal plane. If the angle was not obtained successfully then 
15 SENSOR_ERROR is returned. Ifthe angle was obtained successfully then 

SENSOR_OK is returned. The arguments for this function is an enumerated body 
segment of a single sensor and also a pointer to the output value. The returntype is an 
enumerated type. 

20 SensorGetCoronalOrientation 

The current value of the angle of orientation of the given sensor in the coronal 
plane is returned by this function. If the angle is not successfully obtained then 
SENSOR_ERROR is returned and ifthe angle is obtained successfully then 
SENSOR_OK is returned. The arguments are an enumerated body segment of a single 
25 sensor and also a pointer to the output value. The return type is an enumerated type. 

SensorWriteCalibrationData 

Calibration data in the input array is written to EEPROM of a specific sensor. 
The QSPISendPacket function is used to send this information. Ifthe command is 
30 successful then SENSOR_OK is returned and ifthe command is unsuccessful then 
SENSOR_ERROR is returned. 

The arguments include an enumerated body segment of a single sensor, a pointer 
to an array of SensorCalibrationDataType that contains the calibration data, and the 
length of the. array of the calibration data is also included as an argument. This 
35 function has a return type that is enumerated. 



m:\SDecifications\100000\10729lDrvreo.doc 



25 



15 



SensorReadCalibrationData 

This function reads all the calibration data of a specific sensor from its . 
EEPROM into the return array. Again if the command is successful then the 
SENSOR_OK is returned and if not SENSOR_ERROR is returned. 
5 The arguments include an enumerated body segment, a pointer to a returned 

array of SensorCalibrationDataType that contains the calibration data and lastly the 
length of the returned array of calibration data. The return type is an enumerated type. 
The calibration data shall be returned in an array of UTNT1 6. 

10 The following functions are considered non-API public functions. 

SensorGetRawADCValues 

This function allows the current value of the eight ADC values to be read from a 
specific sensor using QSPISendPacket. The argument is an enumerated body segment 
of a single sensor and this function returns an enumerated type SensorErrorType. The 
eight raw ADC values are returned in a variable parameter passed to the function as an 
argument This variable parameter is apointer to an array of eight UINT16 values. 

SensorAccAXGetCalc 
20 This function obtains the calibrated value of the acceleration measured in the X 

axis of accelerometer A on a specific sensor.. Again if the command is successful then 

the SENSOR_OK is returned and if not SENSOR_ERROR is returned. 

The arguments include an enumerated body segment of a single sensor and 

INT16 which is a returned acceleration 2048 = lg being 2048 bits representing a force 
25 of lg. The function returns an enumerated type and the acceleration is returned in a 

variable parameter. The actual value of the acceleration is obtained by dividing the 

integer value returned by 2048 to give the value in g's. The returned acceleration is 

valid from -2g to +2g. 

30 SensorAccAYGetCalc 

This function obtains the calibrated value of the acceleration measured in the Y- 
axis of the accelerometer A on a specific sensor. If the command is successful then 
SENSOR_OK is returned and if not SENSORJBRROR is returned. 

The possible arguments are an enumerated body segment of a single sensor and 
35 INT16 which is a returned acceleration 2048 = lg. This function has a return type 
which is enumerated. The acceleration is returned in a variable parameter. The actual 
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value of the acceleration is obtained by divide 

give the value in g's. Again the returned acceleration is valid from -2g to +2g. . • 

Sensor AccBXGetCalc 

5 ™ s *"«ionobtaimmecahW 

<UI,S ° f »^ta««*«r B on a specific sensor, As with the previous function if the 

ZZTXT? * m SENSOR - OK is "* ^ SENSOR ERROR is 

M»l Tins Inchon has the same arguments as me prevfous funcifon with the same 
type and denvanon ef to acceleration and range of the accelerate is me same 
10 as the previous function. 

SensorAccBYGetCalc 

«l. ™ S ,* mCtion ob,atas *• value of the acceleration measured in the Y- 

1 , Tf aC ^T iaeI B ° D " SPCCiflC "« Sfailall y reference to the previous 
15 toeUonSENSOR.OKisrehn.edifmec^nnnandissuceessfu.. If me conunandT 
unsuccessful then , SENSORERROR is returned, it has the satne types of argils as 
the prevtous two functions and the same return type. Furthermore the derivaCoflr 
aecelerahon and the range in which the returned acceleration is valid is me same 
previous two functions. 

20 

SensorGyroGetCalc 

This function obtains the current value of the angular velocity of a specific 
sensor and again if the command is successful then SENSOR OKis returned and if not 
successful SENSOR.ERROR is returned. ~ 
25 The arguments include an enumerated body segment of a single sensor and a • 

returned angular velocity 8 = 1<> per second under code INT16.. The return type is an • 
enumerated type and the angular velocity is returned in a variable parameter. The 
actual value of the angular velocity is obtained by dividing the integer value returned 

30 or* ^ ^ SeC ° nd ^ ^ osc °P e retu ™ values valid between +90 and 

30 -90° per second. 

Finally there is one private function which is the SensorGetSerialNumber . 
which returns the serial number for a given enumerated body segment. It returns the ' 
replacement sensor serial number if the enumerated body segment matches the replaced 
enumerated body segment The argument is an enumerated body segment of a single • 
sensor and the return value is a serial number of the sensor at the given enumerated 
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body segment. It returns a serial number of 0x00000000 if there is no device at that 
enumerated body segment. 

The following description relates to software in the microcontroller 46 of any 
one of the sensor packs. The software is stored as code in a memory module, such as a 
EEPROM in the microcontroller 46. Particular aspects of the software include the ' 
slave application layer which includes the implementation of various commands,* 
calibration method used for the sensors, a device specific background task and a device 
specific initialisation. 

As noted earlier, the primary function of the sensor pack is to measure angular 
orientation of the lower limbs and. torso. The software in me sensor packs is divided 
into two portions, the communications protocol and the software that handles the sensor 
specific commands from the controller 10. The controller 10 sends sensor commands 
via the communications software to the sensor software where a command will then be 
performed and, if necessary, a response will be returned to the controller 10. 

The primary function of the sensor pack will be implemented by software on the 
sensors themselves. The sensor packs keep track of their current orientation and the 
controller 10 then sends a command to read the current value of this information via the 
communications protocol. 

Shown in Figure 16 is a block diagram of the various sensors linked to the 
controller 10 through SPI buses 44. Specifically the torso sensor pack 30 is directly 
linked to the controller 10. Each of the left thigh sensor 34a and left shank sensor 32a 
is linked on the one SPI bus to the controller 10 as is the right thigh sensor pack 34b 
and right shank sensor pack 32b. Each of the sensor packs are regarded as SPI slaves 
and the controller 1 0 is regarded as a SPI master. 

In Figure 17 there is shown an implementation of the sensor pack firmware 
which includes various protocol stack layers and various modules to be described 
hereinafter. 

The analog input sampling process 200 constantly samples the analog inputs 
corresponding to four accelerometer outputs, that is AX, AY, BX and BY and also one 
gyroscope output, one strain gauge output and two pressure sensor outputs. The current 
values are then copied into memory. When the controller 10 requests a specific value 
the memory location is read and transmitted back without having to waif for a fresh 
analog to digital conversion. It enables an increase in speed at which the controller 1 0 
can read specific values from each of the sensors. The process effectively is an. 
interrupt handler. The analog to digital conversion must be initialised by calling the 
InitAnaloglnputSamplingO function which is called as part of the QSPI device 
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specific initialisation in the function InitDevice 0 at reset. The most recent analog 
outputvalueswillbelocatedinRAM. The high byte and low bytes- are located in 
separate arrays Interrupts must be disabled when reading from these values to prevent 
partially updated values from being read. 
5 A further function that is used which is also an inteirupt handler for the ADC 

Conversion Complete Interrupt is the function AnaloglnputSamplinglSR This • 
^n^ 1 ^ CUnmt ValUe t0 mem0ry > Ganges the ADC channel register 

^T^!° n6Xt Chaimd t0 ^ ""^ TW ° 8 b * e « ™> as buffers 
10 f^^^^^^^ADCchannel, One array is used for the high byte 

ol^T.n" 8 ^ 01 * 61 "^ Each member of me array correspfndrto 
; h^H^ CCh — ^^--^isimplementedm^ . 
handler by disabling mterrupts while the global memory is being written to 

of » n ahT? &e i fimCtion ^naloglnputSample allows reading of the latest value 
of an ADC channel automatically. Interrupts are disabled while both the ADC bytes 
15 are being read. J 

The slave application layer 202 in Figure 17 is the micro-controller code that 
receives commands from the controller 10, carries out the required action, then 

o~3 ro S tereSPOnSe ' ^^---nti^executeinthecontext 

20 . ™^ Cti ° n0fthe ^^ 

controller 10, perform me desired action and respond. The following functions are 
performed: 

are read^^ 8 ^ *° Wrillm by A^mterrupt 

25 TT S , SenSOR5 ^ ^ reCCiving appropriate command from 

dxe controller 10, the sensor components such as accelerometers and gyroscope will be 
turned off;. . 

turning on the sensors, wherein upon receiving the appropriate command from 
Oe controller 10, the sensor components such as accelerometers and gyroscopes will be 
^>U turned on; 

self testing of sensors, whereupon receiving the appropriate command from the 
controller 10, the sensor components such as accelerometers and gyroscope will be 
tested to see if they are producing an output; and 

calculations on input values wherein the calibrated physical values of 
35 acceleration, angle and angular velocity are calculated. 
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A particular message sesPcktArrive is input to the slave application layer 202. 
This message is processed by the function ProcessDevicePacket called by the session 
layer. Other messages which are input to the slave application layer being appDATA, 
appCMD and appDisconnect are processed as part of the first mentioned message 
sesPcktArrive and hence within the function ProcessDevicePacket. 

All of the commands and messages from the master application layer are 
handled via the function ProcessDevicePacket, which decodes thepacket and takes the 
appropriate action and then sends the correct response/data. The function first looks at 
packetData[0]. The most significant nibble indicates the network address and the least 
significant nibble indicates the packet type. If the network address is equivalent to the 
current network address of the sensor then the packet processing continues. If the 
packet type is a sensor command (appCMD) then the following byte indicates which 
command is being sent. If the packet type is a general data packet (appDATA) then a 
simple response packet is sent back. There are no commands configured to use this 
packet type with sensors. Any other packet type is not processed by the 
ProcessDevicePacket function. 

If a sensor command has been sent, the function looks at packetData [1] and 
compares it to the enumerated type SensorCommandType. Then the appropriate 
command is executed and an appropriate response packet is. returned using the ' 
sesPcktSend function in the SPI session layer. When responding, a different packet 
type appRESP (sensor response) is used. 

The ADC module contains an interrupt handler that copies the analog input 
registers to a memory array after each conversion, as mentioned previously. The 
application layer module 202 accesses this array via an extern declaration. The . 
GetAnaloglnputSample function is not used to return the raw ADC values as this is less 
efficient than sending the bytes separately without combining and splitting a 16 bit 
value first Two different byte arrays of eight bytes each are provided, with one byte 
containing the high byte from the analog to digital conversion register (ADCH) and .the 
other containing the low. byte from the analog to digital conversion register (ADCL). 

The parameters are stored in the memory of the micro-controller 46, typically a 
EEPROM, and are accessed via the session layer. The shaded EEPROM data is present 
on all QSPI devices. The parameters include analog X and Y values derived for each 
of the accelerometers A,B at quick calibrate values -lg, 0g and +lg, providing a total of 
twelve parameters. It also includes the parameter of the gyroscope Odeg/sec value. 
Commands from the master application layer to the sensor slave application layer 
correspond to the parameters such that they control the reading and writing of the 
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parameters : stored in memory. The values stored to memory are loaded to RAM at 
start-up and when the UPDATE_P ARAMS command is received. The read and write 
c™!* ma. read and write the values from me memory EEPROM returns mem te 
me eontroHer umtlO. A typical command may be RD_QCAL AXL for example that 

ZtlteT Ca " brate ValUe - lg f ° r fc ~*~ AualogXvalue, would fri^er 
the following sequence of operations: 

1. RD_QCAL_AXL received; 

Ld fi ^ eadthetWObytes *» *• appropriate EEPROM address, the high byte being 
10 3. Send back an appRESP to the command 

£££££ EEPROMbymeconhderum, 10. Hence me confer 10 Lupdate 

UPD^E ^^^^""^^no.beupdatedunnl.he 
UPDATE_PARAMS command is sent from the controller 10. Most EEPROM 

a C c=essre S s ", mP ^ tafteW,iCati0ntayer - commands 
access the senaf number, QSPI Device Number and Software Veraion Number which 

are nnp.emented in me session layer of the communications software 

bv the T> ' e m ° V, } ne f esai ^ m rel "<* <° calculated values which are perforined 

20 three dunensmm. The calculations areperfonned in the context of the main due*! 204 
manser-defined Background Task 203. The Background Task 203 is a generic 

« 20 mT b" , T Pro0eSSing ' ™ S ,0 ° P md *• ■"*•»- 
25 tate^ntbTrf. ^"'^^'■yanysys.emtatetrup^wbiled.erespective 
me^ *T Weputposeofme sensomismme^fhemienteLof . 

fte sensors m space. The sensors have the capability of culcuhting their ownangular 

Z^T ™ * e , CaUbrated *- *• ~pe and the acceleremeters Sn be 
eclated. The fohowing describes me method ofcaUbrauon and the use of the 

30 MowT 0 " ^ * <al0U,ate * C ^ ° f "» Vari ° M »«-«" 

Accelerometer AX-axis Acceleration; 
Aocelerometer A Y-axis Acceleration; 
Accelerometer B X-axis Acceleration; 
Accelerometer B Y-axis Acceleration; 
35 Gyroscope Angular Velocity; 

Angle of the sensor measured in the sagittal plane; and 
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Angle of the sensor measured in the coronal plane. 

The angular orientation calculation is carried out in the function 
CalculatePosition. This function is called hy the DoBackgroundTask function which in 
turn is called in the main thread 204. Other calculated values are calculated by the 
5 following functions: 

ExecuteCMD_RD_AXcalc 

ExecuteCMD_RD_AYcalc 

ExecuteCMD_RD_BXcalc 

ExecuteCMD_RD_BYcalc 
10 ExecuteCMD_RD_GYROcalc 

ExecuteCMD_RD_ALLcalc 

ExecuteCMD_RD_ANGLEcalc 

ExecuteCMD_RD_CORONAL_ANGLE 

The above eight functions are called from the function ProcessDevicePacket.. . 

15 The calculated angle, acceleration and gyroscope values assume that the sensors 

have been calibrated in the following way. The sensors can be oriented in six different 
ways, once for each of the X, Y and Z axes and another three ways corresponding to 
the opposite orientation of these axes. The sensors are placed in each of these six 
positions which exposes each of the sensors to +lg and -lg on each of the three axes. 

20 The six different orientations are shown in Figure 18. 

The calibration algorithm is implemented externally to the sensor pack 32, for 
example. The controller 10 uses the SPI bus 44 to command the sensor 32,.read the 
data and write the appropriate calibration constants to the memory unit in the micro- 
controller 46. The calibration of the accelerometers 50 and 52 are carried out as 

25 follows: 

1 . The sensor 32 is placed in a first position, held still, then 32 consecutive values 
are measured for all of the accelerometer and gyroscope ADC inputs. 

2. The median of all the values for each ADC measurement is found. A noise 
resistant estimation of the calibration value for all of the accelerometer and gyroscope 

30 ADC inputs for this position is determined. 

3 . The medians in a row of an array are stored. 

4. Steps 1 to 3 are repeated for each of the six positions, creating six rows. 

5. Each column of the array of medians of the accelerometer values is searched for 
each of AX, AY, BX and BY. The minimum median measured for each corresponds to 

35 the -lg level. The maximum. corresponds to the +lg level with the 0 level being 
calculated as. the average of these two values. 
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«. All measured gyroscope values correspond toO- per second. The most recent 
medtan value is used to se, the gyroscope zere point This sets toe zero potabZ 
as me gafa is not set to this procedure. Pomtontnot 

.5 controller 46 ,n the sensor 32 using commands previous mentioned 
8. The sensor 32 is now calibrated. 

IN. ft ^. aCCel " rometere 50 -* « « able todetec, the acceleration due to gmvity 
Ttas factor ,s used to calibrete each of the sensors. When the sensors are plaJTa 

.0 t 6 ^T" eXPerienCM by a-leremetL wi„ be 0g 

10 ,f toe ax* „ honzontol with respect to ground. If me axis being measured is vertical 
wrth respect to the ground men the accetorauon experienced win be +I g„_l7 

lXe" ,80,,,heO T ,,a,iOnOf,heSenSOr - ^a 110 * ^ Placing the sensor 32 on a flat 

measunng the accelerations and then placing the sensor upside down one axis 
^^"tsmaybecaHbratedby^ 

^^^'-es may be calibrated in „ is manner. The Sensor * 
Harness used for calibration requires that the sensor 32 be placed in me six pos^e 

~ ° f — - ™- «* - whichTalr 

20 ti, , y sw * fferent P° sltl0ns arerepresentedinFigurelS; 

The acceleration measured by each of the accelerometere 50 and 52 is measured 

samphng me mputs, leavmg toe curten, voltage measurement in memo,,. The voltage 
measured » represented as a ten bit unsigned integer stored as two unsigned 8 hi, 
b urtegers. Ti, followjng fennnli u to ^ ^ J*J»« 

a = k{A raw -A 0 ) 

A„-A L 0) 

where A ra „ = the 10 bit raw ADC value for one of AX, AY, BX, BY. 

Ab - the 1 0 bit calibration 0g value of AX, AY, BX, BY.' 

A H - the 1 0 bit calibration +lg value of AX, AY, BX, BY. 

A L = the 10 bit calibration -lg va i U e of AX, AY BX, BY 

k- -scale factor. The output is a signed 16-bit integer where a value of k/2 
corresponds to lg. 

a ^.acceleration, in g's multiplied by k/2. 
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The acceleration is returned as a signed 16 bit integer in 2 bytes, using k = 4096 
so that 2048 is equivalent to lg and -2048 is equivalent to -lg. The form of the 
equation (1) is chosen to minimise the quantisation and round off errors due to integer 
arithmetic. s 

Regardingthe calculation of angular velocity, a nominal gain for the gyroscope 
«gnal^nditioning circuit of 6,25 is assumed. Therefore ±90<> per second responds to 
± 0 625V around the zero angular velocity voltage. The value 3.3V corresponds to an 
analog to digital converted value of 1024, with 3.223mV per division and a 10 bit 
resolution. 

The angular velocity is represented by the following equation: 

<»=k(GRAW-Go) 

- deg.^- 1 (2) . 

1 5 where Graw = ADC gyro value. 

Go = Gyro zero value estimated using a median filter 
k = 8192 

kg = a value calculated using a previous method 

20 second^ 8 ^ & ^ * ^ ° f 8 corres P^ as to ^ 

If co = 8, the angular velocity is +1 7s 
Similarly, if co = -8, the angular velocity is -l°/s 

The parameters k and kg have been chosen to ensure that using integer maths to 
calculate equation (2) minimises errors due to rounding and quantisation. 

The sensor 32 combines information from both the accelerometers 50 and 52 
and the gyroscope 48 to calculate the current angular orientation of the sensor with 
respect to gravity. A signal processing technique called "Wiener Filtering" is used to 
combine the accelerometer values and the gyroscope values to get a better estimate of 
the sagittal angle than by using either estimate alone. Reference is made to the text 
30 ^duction to Random Signals and Applied Kalman Filtering" by R G Brown and 
PYC Hwang, 3 edition John Wiley & Sons, ISBN 0471128392. This technique 
utilises the fact that the gyroscope generated angle (integrated angular velocity) is good 
for dynamic situations and the accelerator generated angle is good for static situations. 
The sensor is designed to measure its angle with respect to the horizontal plane along 
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its axis Angle meamtement by the sensor 32 is shown in Figure 19 . Further 
unirmahon on Wiener Filtering is provide toAppendixl 8 ™ * FuIther 

due to ™ e ^ lerometos 50 52 can be used tomeasure the acceleration vector 
accelerometer A or B as shown in Table 1 below. 




20 



nTT aZ^^TT H 0WCTer ' ° nly ° f freed ° m ^ "° °^ned 

^Ttl" ^ "earing- cannot be determined ftont the gravis acceleration 
15 Xed te Ltrt , T ^ ""^ *" * «—L vector are 

— P r:^ ttaiandcoronaiaa8,M - — . 

^•^•^••^can^-^V««al^«dd» MWI>Mk i, ft in 
the acceleration measured by the sensor pack is given as: 

a* -gcos(j>)sin(0) 

<i, = ysin(«>) 

<»j = gcos({>)cos(0) 

va I ueso^ P T°!, the ^ ** ^ " ta *• — — 

Tn^ L T **. Parame,er 8 " * e magni,nde <*—«*» due te gravity. 

wL I : * ', " tata " 1 on a conanuai bMis —« *• — — -rvalue 

rerls IT ° f V8 fe Iimi,ed '° ° M » to *• -era e sine (unction 

returns a valtd answer. The coronal angle lies in the range of-90- < 9 < 90« 

The sensor angle or sagittal orientation 6 may be calculated flom either the 

ZtTeT °7 TT™* previous,y refOTed to OT "» «*— — >ts 

30 unTeteTJ y , **** — ' - "° over 

tune to obtam an estunateofme sagittal angle. The detected acceleration can be used 
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to calculate the angle by assuming that the acceleration is due only to gravity, and by. 
using trigonometry to calculate the sagittal angle 0. 

Both of these measurements are not always correct. When the sensors are 
moving, the total acceleration measure consists of two components, being, gravity arid 
5 acceleration due to the motion of the sensor, that is: 

ai = g + a m 

where a» is the total acceleration vector measured, a m is acceleration vector due 
to motion of the sensor and.g is the acceleration vector due to gravity. 

10 Hence the angle derived from the acceleration values contains errors due to the 

movement of the sensors. The angular velocity measurement and its derived angle is 
substantially accurate when the sensors are moving. When the sensois are still, the 
angle derived from the accelerometers is reliable. However, the integrated angle 
derived from the angular velocity measurement is susceptible to accumulating error due 

15 to small offsets in the calibrated zero for the gyroscope. 

The angle 9 shown in Figure 19 is derived from the acceleration values by using 
a 4- quadrant inverse tangent function. The 4 quadrant version of the inverse tangent 
function is used so that the full angular range of -180° to +180° can be derived from 
the measured accelerations using the equations given for a x , a y and a 2 in (3) above". 

20 The angle 6 in Figure 19 is derived from the angular velocity values by 

integration. A trapezoid method approximation of integration is used to do the 
numerical integration. This method has a particular advantage of allowing variable 
time steps which is useful as the angle calculations are running in the background with 
variable timing. 

25 The particular equations are: 

tn 

On = 0„-i + \<oUdt 

tn-1 

- On-l +(©„ + ©„.,) f . 

M»- t„.i) 

30 i 

The sagittal angle value 6 returned by the sensors is a signed 16 bit integer 
scaled so that 1 28 corresponds to 1 °. The return value is -250° to +250°. 

In.appendix 1 there is a description of Wiener filtering in relation to estimating 
the sagittal angle from the two angle estimation sources above mentioned. 

With regard to the implementation of the filters there are two first order filters 
in the estimator (referred to in Appendix 1) that must be efficiently implemented in the 
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sensor micro-controller 46. The filters are implemented as discrete filters assuming a 
samphng rote of 128 H z whioh is the average analysis rate in the implemented system . 
The filters are transformed to the discrete time domain nsing the bilinear 
transformation: 



-i 



s = 2F r 1-2 
1+z- 1 



(4) 



10 



where F s is the sampling rate. 

The transfer function is converted to difference equations as follows: 



Y0) = J_ X(s)=>y t ^ 

TS+1 



2F,r-l 
2F s x+ I 



15 



20 



y iA + x ' + x " 
2F s r + 1 

(5) 

The filters are implemented using integer multiplications and divisions with the 
input being multiplied by 16. The value of the output of each filter is 16 times the 
actiud filter value and this reduces the effects of excessive quantisation in the 
calculation of the filter outputs. 

The sagittal angle estimation algorithm must be initialised soon after the. sensor 
is activated by turning on the gyroscope and the accelerometers. The function that 
hirns on the gyroscope calls the InitCalculatePosition function when the gyroscopehas 
finished imtialisation. The InitCalculatePosition function initialises the integrator and 
filter so that the output.of the sagittal estimation algorithm is an angle derived from the 
accelerometers. 

As mentioned previously a built-in test runs continually when the sensor 32 is 
activated. The built-in test causes the LED to flash quickly when- the test fails, 
mformmg the users that the sensors should not be used. The test also fails under the- 
foliowmg conditions: 

1. If the long tenn average, calculated by the pseudo-median filter described in 
Appendix 2 of the gyroscope ADC value falls outside a certain range of the calibrated 
0°/second value (within 50 ADC levels); 

2. If the redundant axes of the two accelerometers 50 and 52 do not match for- a 
significant period of time, that is outside 0.25g difference for 400ms; and 

3. If the gyroscope signal is stuck at a constant voltage. 
35 ™ sl ^ <^ition is a lmo^^ 

shocks exceeding its maximum rated shock. This failure mode is detected when the 
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^gitW angle estimation derivative is non-zero for a period of time, typioallv greater 

than 400ms, while the gyroscope signal is at a constant value. 

The built-in test conditions also fail when the sensor is not ealibrated or there is 
5 * 7^ lm Wi ' h 11,6 « gyroscopes. When a calculatod value 

.5 road by tire controller 10 and fhe. built-in test Mure could afleet the value to bTZ 

then a bmlt-m test etror byte is refuroed along with me eutrent value .^'^ 

on me ^ >^*>«-«- filter used are in Appendix 2 and former details 
on the gyroscope cahbration program is given in Appendix 3 

10 anv OS^r 0 "* 18 ^ * deScription ° f ^ «f *e nucro-controller software on 
2L « Z i FurthemMre oo-mnications protocol usefi by me micro- 
eontioller « is also described. Non-device specific firmware in any QSPI delHs 
2 desorbed b^mafrer a*, indudes s,ave QSPI prbfoco, mcludmg ilonT^ 
dammAlayerandphysicaflayerandalsomeslaveapplieationlayershell 

ts • k. . R f mn S backt ° F *S*> 16 each of the devices 32a and 32b being the left and 

dev.ce 30 bemg the torso sensor are considered to be QSPI devices and SPI slaves The 

tire conn-oiler 10 to communicate with a number of QSPI devices using me QSPI 
device and communications pM An application in the contre..ef 10 2nL 
20 commands to each device apptication using fids protocol and the slave oevTce L^ 
this command and returns date if appropriate. uevice performs 

laver Sk**T 21 " ' dia8ran ,6 KgUre 17 includ « * session 

layer taction 205, a data link layer function 207 and a physical layer function 209 

25 rrSir f r 209 r a s,ave ,ayer ** ■* «^-« -oC 

oufft f I T to micr °-«» lrol '« «* This layer handles the circular 
buffer for transmitted or TX and received or PYi* -n. "rouiar 
the<!Plt™™i ■ . •™ areoe,veUorRx o«a. The operations are executed in 

the SPI transmission complete intemipt handler and the main thread 204 

tw-fi^ 7*" ' S enhy P ° tal f0r "» Wtialisation,' QSPI Device 

mlfi^r* " d QSK dCTiCe "*ta . The latter 

.mtiahsation function is provided by the QSPI Device Speoific firmware. The second 

~— ti QSW - ^ SP6CifiC - k * — « o 

35 E^T- !, r ^ »«"*■«''—». bytes from the physical layer 
35 buffer is earned out in the main thread 204. .. 
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The data link layer lunette, 207 is also a slave layer and reeelves and transmits- 
variable length packets with error detection. This entity executes in the context of the 
man, thrend 204 and the operations of this layer are described more fully later in the 
context ofQSPI communication protocols. 

5 confix ^ SeS T ^ & " Cti0n 205 * ^ ' sUve «** controls link 

«nfigurat,on and management and acta as a conduit for the application layer 202 

commands and responses. This entity executes in the context of the main thread 204 

and the operations of this layer are described more fully in relation to the QSPI 

10 ZZTT" Pr ° tOCOlS ^ -n*****-"* ** also handles QSPI Devico 
10 Idenhflcatron commands^ some other non-device specific functions 

con* T'T" 0 " 1 ^ 202 h * SlSVe layer fimoao " whi <* * «" "uc™. 
conhoner code mat reaves commands ftom the controller 10, carries out the required 
achon and then transmits the appropriate response. 

15 bei„» „ Inte T ° f T Stn,CtUre "* °° de &r my QSPI is ""vided into two parts, 
15 being device dependent and device independent 

The main thread function 204 has for its purpose processing communications 
and executtng file device specific background bask. The functions of the main thread 
204 are to perform global initialisation of me processor, call any device specific 

20 con™ « T" 8 ^ baCk8ro » nd and. Process 

ZZFTZ£ ~ * 6 fimCa ° nS ,DitDevi0e - ^Backgrounder are 
defined m me QSP, devico speoific code. The initialisation vdues are written to their 

■ ^-^'^-atsttutupandonreset. At reset, the processor is initialised 
usmg tte ImhahseProoessor mnetion to be described hereinafter. The main thread 204 

25 ZTk r r ' 00P ** handlK «--»«™<=aaons processing and the devico 
25 specific background task. 

The InitialiseProcessor function has no arguments or a return value type as is 
the case w,th the main thread function or entity. The InitialiseProcess function 
performs global initialisation to enable global interrupts, set the RAM page, disable 

30 SRaL TV* t0 be DOt SleCping aod make SUre *™ is no vernal 

kyer ' link ^ «* session layer initialisation functions are 
called from this function directly QSPI Device specific initialisation is carried out in 
this function by calling InitDeviceO, defined elsewhere in device specific code. 

The main thread 202 as mentioned previously consists of an endless loop that 
calls a particular function in the data link layer 207 function when there are 
unprocessed bytes in the receive buffer. If there are no bytes in the receive buffer to be 
processed then the device specific background task function will be called 
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The slave physical layer 209 is a communications protocol layer and its function 
is to allow basic communication with- the controller 10 using the SPI of the micro- 
controller 46. The physical layer also provides an interface between the micro^ 
controller SPI and the higher communication protocol layers. The operation of the 
5 physical layer assumes that the layer has been initialised using the ImtPhy^icalLayer 
function defined in the Functions section of this entity. The physical layer is 
implemented on the micro-controller 46 configured to be in slave mode communicating 
on the SPI bus. . • 

The physical layer has two situations where processing must be carried Out that 
10 is sending bytes, and receiving bytes. The layer- provides a software implemented 
circular buffer on input and output data from the SPI. The SPI Complete interrupt 
handler attends to the receiving of bytes, storing the bytes in the RX circular buffer arid 
transacting bytes from the TX circular buffer. The processing of the bytes occurs in 
the main thread as previously mentioned. 
15 When a new byte arrives in the SPI data register or SPDR, the SPI interrupt is 

raised. The SPI interrupt handler reads the incoming bytes into the physical layer 
circular buffer. When the interrupt handler has finished execution the main loop or 
thread 204 resumes and the bytes in the circular buffer that haven't been processed are 
then processed by a called particular function. The function call corresponds to a 
message from the physical layer to the data link layer indicating that an SPI 
transmission has just occurred. When the SPI interrupt handler is called and there is a 
byte in the TX circular buffer waiting to be transmitted, then the next TX buffer byte is 
written to the SPDR after the SPDR is read. 

Three functions defined in the Functions section in particular are used - in the 
25 slave physical layer being InitPhysicalLayer, dlSendAndReceiveByte which writes a 
byte to the circular buffer for sending on the SPI bus 44 and returns the last byte 
received directly from the SPDR. Furthermore there is the function SPI_ 
SenalTransferCorripletelSR which is an interrupt handler triggered every time the byte 
arrives on the SPI bus. 

30 The slave data link layer 207 is a communications protocol layer that sends and 

receives packets between the controller or master 10 and the micro-controller 46 which 
is the slave. Error detection is also carried out. The layer 207 receives messages from 
the three sources, the physical layer, the session layer, and the master data link layer: 
These messages are implemented as C functions that manipulate . the data of the slave 

35 data link layer. The functions that are used as an interface to the DLL .include 
plTxFinish, sesPcktSend and ImtDataLinkLayer. 



20 
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The function sesPcktSend acts as a message to the DLL from the session layer 
The function turns the input data into a packet by adding a start byte, a length byte and 
a checksum. The packet is also byte stuffed which involves the process of replacing 
eachstartbyteinthemessagebytwoconsecutivestartbytes. The function corresponds 
5 to a message from the session layer to the data link layer instructing the DLL to send a 
packet to the master device. As air communications with the controller 10 are 
confrolled by the controller 10, this means that all the bytes of the packet are written to 
the SPI data register by the physical layer until they are all read by the controller 10 * 

in KyfTcrt ^ ^ CUmmt DLL ^ iS ^ men *e blowing actions take place, the 
10 MISO output tri-state buffer is enabled, a checksum is calculated, the packet is byte 

staffed, the packet in memory is assembled and the DLL state is changed to FR_SEND 

The formatted packet is sent using repeated calls to the dlSendAndReceiveByte 

functioa If the current state is not IDLE then nothing happens. 
Wjm regard to me function pl^^^^ 
15 Phy*cal layer. When the byte received is a start byte then this corresponds to 

plStartByte otherwise this corresponds to plByteArrive. 

The function plWriteCol acts as a message from the physical layer. It is called 

when there has been a write collision and handles the correction of that collision. The 

DLL state is changed to IDLE. 

20 The function InitDataLinkLayer is called and reset as part of the- processor 

imtiahsation. In order to initialise the data link layer, the timer is stopped, the data link 
layer buffer and global variables are cleared and the timeout values are intialised 

The slave session layer 205 is a communications protocol layer that functions to 
link configuration, link management and handle errors. The session layer 205 receives 

25 messages from the slave application, the slave DLL and the master session layer. The 
following functions act as messages to the slave session layer 205 from QSPI device 
specific firmware: 

appSendPacket and appDisconnect. 

The first mentioned function, appSendPacket, acts as a message from the 
30 application layer to the session layer. As this is a slave, this function is called when 
responding to a command or message from the master application layer (QSPI Driver 
in the controller 10). This function is a more general packet transmission function that 
can be used for transmitting any packet type from the QSPI device. It does not have 
any return value types and the arguments include a four bit network address, a four bit 
35 packet type, a data message length being the length of the data portion plus any- 
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command bytes of the packet and finally the command bytes together with the data in 
the packet. 

The function appDisconnect acts as a message from the application layer to the 
session layer and allows a logical disconnection from the master controller 10 so that 
5 the connection may be reinitialised. It has a network address as an argument and no 
return value types. 

The following functions act as messages to the session layer 205 . from non- 
QSPI device specific firmware: 

sesAnyResp ' 
10 sesReqInfoSet ' 

sesConfinnCon 

sesTIC 

dlTimeout 

dlPcktBad 
15 dlCollision 

dlPcktReceived 

The functions sesAnyResp, sesReqInfoSet and sesConfirmCon act as messages 
from the master session layer to the slave session layer and are used in auto 
configuration. None of these functions have any arguments or return value types. 

The function dlPcktBad acts as message from the datalink layer to the session 
layer. This message is sent if there is a. received packet with a detected error. The data 
link layer is then changed to the idle state. The function has no arguments or a return 
value type. 

The function dlCollision acts as a message from the data link layer to the 
session layer. The message is sent if there is a received packet with a detected Write 
Collision. The data link layer is then changed to the idle state. This function has no 
arguments or a return value type. 

The function dlPcktRecieved acts as a message from the data link layer to the 
session layer. This function is called by the data link layer when a. valid packet has 

30 arrived. The packet is passed to the session layer for processing and the function 
implements the appropriate actions as described previously in relation to the QSPI 
communications protocol specification. This function sends the error checked packet 
to the application layer by passing a pointer to it. It has not return value types and it 
has two arguments one of which relates to the length of the packet received and the 

35 other being a pointer to the data that has been received. 



20 



25 
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. The session layer has one timer that is used for timeouts and this is preferably 
an eight bit timer. y 

The slave application layer is a QSPI Device Application and has for its purpose 
to meet, the functional requirements of a specific QSPI Device that is using the QSPI 
5 communications protocol to receive and process commands from the controller 10 

Therefore the function of the application layer is to receive commands from the 
controller 10, perform the desired action and respond. 

As mentioned previously the sesPcktArrive. is essentially a message to me slave 

10 Z^T l u Y T • ^ C ° mPriSeS SUb - COmP ° nentS a <* Data whichis * 8-eral data packet 
10 appCMDwhich is.an example of a QSPI device specific command, being a sensor 
command packet, and appDisconnect which is a command from the master controller 

The function ProcessDevicePacket is called by the session layer to process the 
sesPcktAmve message. The three sub-messages above are processed as part of the 
sesPcketAirive function and hence in ProcessDevicePacket. 
15 With regard to the function ProcessDevicePacket it has two arguments, *he. of 

winch relates to the length of the packet received and the other being a pointer to the 
data that has been received. It has no return value types. All the commands and 
messages from the master application layer are handled by this function which decodes 
the packet and takes the appropriate action, and then sends the correct response/data. 
20 Tins function first looks at packetData[0]. The most significant nibble indicates the 
network address and the least significant nibble indicates the packet type. If the 
network address is equivalent to the current network address of the QSPI Device then 
the packet processing continues. If the packet type is a specific QSPI Device 
command, for example, sensor command appCMD, then the following byte indicates 
25 wmch command is being sent. If the packet type is a general data packet, such as 
appDATA, then a simple response packet is sent back. Any other packet type is not 
processed by the ProcessDevicePacket function. 

Assuming that a QSPI device command has been sent then the function 
packetDatafl] is read and compared to the enumerated type that defines the various 
30 commands for that device. The appropriate command is then executed and an 
appropriate. response packet is returned using the sesPcktSend function in the SPI 
sesswn layer. When responding a different packet type is used. 

The InitDevice function has no arguments nor a return value type. This 
function is called at reset from the Processor Initialisation function. It performs any 
35 initialisation of the QSPI Device that is device dependent 
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It ^ D ° BaCkgrOUndTaSk ds ° has no or a return value type. 

It performs any device specific background task in the context of the main thread 204 

W T Cati0nS Pr6C6SSing Wi " bC eX6CUted in Preference to «• in each 

5 ™ fh r ^ ^ 00ttta "^ ^ ^ bett * ^ " does not tie 

up flxe processor for too long at each call. While it is executing, new bytes that have 
been received cannot be processed. 

the cn ^f 11 ^ des ^Ption relates to the QSPI communication protocols between 
the controller 10 and each of the QSPI Device, As mentioned previously the 

1 0 Z^ZZZ ^ ° f *"* kyeiS ^ *» PhySiCal ^ ^ link 

I^LT r SWn yCT - Each «* these layers provide a means for the master 
application in the controller 10 to communicate with the QSPI Device Application 

The physical layer is the SPI link between all of the sensors and the controller 

1 « "TTr* 8 bCtWeen *• C ° ntr0Uer ° r maSter 10 — one QSPI device or slave 
at a to. All the communications are controlled centrally by the controller 10 

cnn^ll ^n data ^! ayer SCnding ^ of P ackete ^ta between the 

ZS-ZZt 0ftheSlaVeS ' JBrror detection is carried out and communication 

The session layer provides for the connection configuration and error handling 

20 l^"™^^ This layer 

Tospth 6 C ° mm " nS interface to the QSPI Device hardware application and 
the QSPI driver on the slaves and the controller respectively. 

The navigator application is the QSPI driver which interrogates each of the 
opemL QSPI d6ViCe mfonnation to control the QSPI Device 

25 Finally the QSPI Device application is a software module in the QSPI Device 

m^cro-controller 46 that receives commands from the controller 10 and carries them 

™pt n h ° Wn ^ FigUre 22 ^ Pr ° tOCo1 layers ° n toller side and the • 

™ J ^ ^ fl ° W b6tWeen &e Various ^ Sh own in Figure 23 is 

30 a block diagram showing the various messages between each of .the layers for the 

master controller and the various slave devices. In the QSPI Devices or slaves, the 

messages are implemented as functions. In the controller the messages between layers 

are implemented as functions with the result return. 

As mentioned previously the session layer provides for the functions link 
35 management, link configuration and error handling in transmission. In addition to 

these it provides for the functions autoconfiguration, sending the application commands 
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to the correct QSPI Device, handling heartbeat messages and Identification messages as 
well as handling programming commands. 

Programming in QSPI Devices is carried out by the controller 10. All the QSPI 
Devices must first be connected logically and prograniming mode is entered when a 
particular message is sentto the session layer. A command is then sent to the correct 
QSPI Device to start progi*rnming. The remaining devices are put into a "sleep" mode 
while mis is occurring. The programming mode is exited by sending a particular 
message to the session layer when in progranuning mode. 

Generic QSPI Device Commands are sent by the master session layer to the 
slave session layer. These commands have a packet, type of "Configuration Packet". 
The slave session layer then handles the commands and returns the required 
information. The particular programming commands are handled by the session layer 
All of the QSPI Device commands are triggered by a message from the application on 
the master or controller side to the session layer. The session layer then sends the 
15 specific command to the slave session layer which in turn carries out the desired action 
and returns the message if necessary. 

With regard to the auto configuration function previously referred to it includes 
a procedure whereby a list of expected sensors in certain positions record the QSPI 
Device information about each QSPI Device. This includes, the lead that the QSPI 
20 device is plugged into, the serial number and the logical address, the logical connection 
status whether it is true or false. 

Furthermore there is a quick configuration procedure which is an alternative to 
the automatic configuration procedure. The quick configuration procedure requires that 
the serial number of the QSPI Device being connected is known in advance. This 
25 procedure is for configuring a single QSPI device.. 

With reference to Figure 24 there is shown the state diagram that shows the 
general format of sending commands to a QSPI Device. When the session layer is idle 
a message is received from the application layer to send a command to a specific QSPI 
Device, in this case the command appCMD. The state changes to SEND and then the' 
30 appropriate packet is sent to the data link layer, using the message sesPcktSend, where 
it will be formatted and sent to the appropriate QSPI device. 

When the packet has finished being sent, a dlPcktFinished message is received 
from the data link layer and the state changes to READ. The session layer then sends a 
message to the data link layer to read a packet from the QSPI bus, using the message 
35 sesPcktRead. When the data link layer has finished reading the packet and the packet 
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is correcdy formatted wittou. any transmission errors, a dlPc^eceived js received 
The response is passed back to the application layer. 

coll.V "if"* m ° f S ° me ldn4 fOT eXample 8 out - or write 

colhsron ttten the session layer tries to send the command N times, currently N " Tu 

ZZZZT — M - ' — — -~ > ~i 

M M « referen<:e *° Ei& * e ' 25 <*>* diagram concerning the OSPI 

Idennntatton Command. It. shows. a sitnatton where a conunand^V^e 

connected" and "connected" These refer to » i« , W 
and tjifl c i a u . 7 - r t0 a ^S"* 1 connection between the master 

whertL? h ? " ^ been exch ^d so that the master device 

20 X^^r^^"-^ Atreset/aQSP^Ji 
Fk« raTZSL r -f State ' A POrti ° n ° f dia ^ « *• left side of 
OSW De T t f ^ ° f 016 aUtomatic configuration procedure. Once the 
QSPI Dev.ce has been logically connected the state becomes C IDLE Th 
layer is now connected and waiting for a command ~ 7,1686881011 

25 will, ^ a P 30 ^ 1 arrives that is addressed to this QSPI Device then the data link level 

LtnecT WWCh mUSt 06 « « ^ ** — « layer then it is exe^ 

and, if necessary, a response is sent back via the sesPcktSend message to the data lint 

30 then i, ^ ^ "J** PaCk6t C ° ntainS 3 not P roce «cd by the session layer 

action 8 p r r r lication ,ayer for - d - <£2z 

ACTION. When the apphcatxon layer has finished the command then it will send a 

message to the session layer to send a resnon^ho^ tt . T 

session.™ a *u ° sena a ^ponse back. Upon receipt of that message, the 

session layer sends the packet back tr. 

ic pawcei oacK to the master controller via the sesPckt^enrf 
message to the data link laver Wh*»n i« «. sest-cktSend 
35 sesAllL inKlayer " ln &C non-connected idle state NC IDLE, if a- 

sesAllRespondNow command is received a nenr ^ . .- 7 

witu „ ^ £ J , received, then the QSPI Device responds immediately 

with a predefined packet that is identical for all QSPI devices. . 
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be „ , T ^ ^ ' ** layer 0,18 aU0WS vari * le P—* of data to 
be sen. and reeved to and from the contreller and read from the slave devices. The 

TZT ***** checksuin eTOr de,Ktion - d » - e °f « 

dlPcirtBad message ,s sent to the next layer above. The ehecltsnm is calculated and 
5 appended to the message so mat when the checksum of the message and checksum byte 
ts^culatedrtshourebeO. The checksum is coated on me lengft and oTh^ 

slave OSP^r b ^ " " 8 diagram deScribes *• *•» ■* "yer in me 
10 I ^^.^^^^^^btoencodeordecodea 

10 vanablelength sequence ofbyteamto.a packet for Emission or reception TheDLL 

which to ££! rt, " *L WU,S " ,iSSi<>n °™ 11,6 ~ *W- »as a start byte, 
15 totoirr ^ WhenaPa ° ketStartS - ^ specific value is used for 

■?££2Z 1 T"* ^ Si,Ua,i0n Wh6le «—»■• <**• P-ker happens 
st^ h ^ *a ® start byte must be considered. When mis occurs, a stogie 

To^r message is ^ •« - -u. - 

The events entailed to receiving the packet are- 
20 The DLL is in IDLE; 

The DLL knows a packet is starting to arrive as a start byre has just arrived ,h„ 
is message plStartByte. The state then changes to FR_jSTART ^ haS J1 ' S ' amved - 

If the next byte is a start byte as well then the length is this value or the packet is 

« starts „ ^ moment h ^ ^ ^ stite STUP ;*~; 

bl a" , • length - TOe ^ tett «^ to ^-Nhavtogreccived U ,^g; 

wt, » T- "f^ 1 10 ^ ** ^ °^ «— - » «- me DLL knot 
when the packet ,s finished. The length byte is written to a buffer 

If a non-start byte anives then the state changes to INC N. The counter is 
rn.emen.ed and the byte is written re the buffer. If a s,ar« bytels J*££Z 

anato !f ^ ^ * ^ ^ " " Msumed ** * e Packet is starting 

35 ZL * ^ " ,eng,h -* ~ - ^ges to REC N. Tne 

STUFF3 and back to INC_N for the next start byte and the counter is increment by 
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one. For non-start byte,, the atate remains in INCN unless the last byte in the paeket 
T ^ f 1881 to 1116 ^ * »« — *- the checksum must 
CRC ™ ^ tk ^ «-'— «■< ^ state ehanges to 

5 • Che ° kSUm lMS been «*«**«» - "Packet Bad" message 

Ltl 18 TL" 3 ** is deceived, iasen. I 

8638100 layer. The state changes to IDLE when the packet has been received ; 

sesPcJ^ ^ 6f T mitttog 8 DLL » 8 the IDLE state and receives a 

sesPcktSend message iron, the session .ayer requesting that a packet is to be sen,. The 
DLL men calculates the checksum and makes the packet, byte stuffing where 

tajft. As the slave QSPI Device cannot initiate a serial transfer of a byte itself, i, 

the contro Her 10 ma, * no, a stert byte, ma, is, a pack* is no, bemg sen, Wth, 
controller ,0, tire SEND stateis entered, me counter is decremented and me next bit 
15 copied to the SPI bus for transmission. menextoytejs 

When all of me bytes have been transmitted tire state retoms to IDLE. Ifastart 
byte .a received while foe slave is transmuting a packe, men fofc m eans mite 
«**r 10 transmitting a packe, also and must take precedence. Thus foe stete 
^^-'cFR-STARTa.dmeDLLmove.mtoa.celvlngpaeketmoae 

•P^ITf foe^T t0 r SUre ^ *" " 8 **» «- *«*- *• 

^DLL h „? 7 COn ' r0 " er la to *»W to is me s^ne as the slave 
Uyer DLL bu var.es due to <he difference in me way conunumoation is initia,ed fiom 
the controller 10 for both sending and receiving 

25 whie* R£Cd, ? SapaCkeUsWtisted,, J' a8 « 8 P'=k«eati message fiom me session layer 
25 whtch causes foe stete to change to POLLING. The controller 10 theu active* reads 
foe SPI bus until a star, byte is received. Tbeu foe remainder of foe packet reception is 
foe same as for foe slave DLL except foatfoe bytes read are initiate! by fte^Uer 

30 which Sen<,in8 8 " by ' "**«*-* — torn foe session .ayer 

whteh 8 tiansmtssion sequence similar to the slave .ayer DLL. The only 

stot?\ 8 " " rra,BmisSi0 ' 1 O"*—- -cssage (p.TxFinish) triggers foe 
state transition to send the next byte. 

»„„/ " W ! I .l be aPpreCiated by P"* 80 " 8 8M1 «1 in foe art that numerous variations 
35 ^ 0 ' modlfi<:atioBS -» "» «-* 'o the invention as shown in foe specific 
embodmumts without departing fix, m foe spirit or scope of foe invention as broadly 
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described. The present embodiments are, therefore, to be considered in all respects as 
illustrative and not restrictive. 

Dated mis 1 6th day of April 2004 

Neopraxis Pry Ltd 

Patent Attorneys for the Applicant: 

FBRICE&CO 
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5.4.6.3 Wiener Filtering 

SXSS^ from ** 2 «** estimation sources gained in Section 54 62 



Angular Velocity 



Integrator 



®mt ® actual ^int 



<!> 



e =e 



'flee 



'ccc 




Low Pass Rlter 




Sum1 



© 

Estimated Angle 



Accelerometer Angle Sum 
Figure 5-4 - Basic Wiener Estimator Structure 

m^'^^n^^ measurement (9*) and the acceleration derived angle 
measurement (G acc ) is calculated. This signal is therefore the difference in the errors in hnth tW 

"XT' ^.-"P*" 1 is - adC ab ° Ut 4,16 <*-™*^ of the two errors wiftre™ to thettetS! 
' S aSeT^^ft^l^ " aSSUmCd l0W freqUency noise " ^ - the !S£KE£ 
foTute d^ftduTtaZ^li * CharaCterisin g e <" as *™ frequency noise accounts 

ror uie dnrt due to a small zero offset with the angular velocity measurement. Characterisine e as hieh 

££SZ£ aCC0Unte ^ £ T ^ ? e acceleroraeter «*WDg due to movemenTSTfenSr^iSt 
^TdS ^ S,gD ^ 5 ' 5 Sh ° WS a re P resen * ation of *» spectral content of the e* and 




Figure 5-5 - Spectral Representation of the Angle estimation errors 

A first onier low pass filter (LPF) in series with a rate limiter is used to extract an estimate of e* t ) from 

^nT^y^V^t? X T 81 " 6 ° f °' 6 iS US6d m LPR ^ of * is c hosen hased on the assumption that 
error in the integrated angle is any component of the estimate difference signal (e^) below about 0 5Hz. 

The rate limiter is set to one of two separate values depending on an estimation of how still the sensor is. A low 




APPENJ> f y i 



& actual - & actual + e int ~ < 



The final estimate of 0 is hence given by: 



?! PO ?^ al OVerfl T " 1116 intemal value e ** can * eliminated by the addition of 2 elements The 

mananglebyamultipleof360»willbeignoredmmedifferLe^ 8 180 . Hence any offset 

no^ fte lS tadly r ^ «?* m *rt*h« angle e acc at ±180°. When the angle is near 180* or -180« 
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Figure 5-6 - Complete Angle Estimator 




Pseudo-Median Filter 

1.1 Variables 

Table 1—1 



Variable 


Description 


M 


Current Median Estimate 


e 


Step size. 


highCount 


Count of samples higher than M+e. 


lowCount 


Count of samples lower than M-e. 


equalCount 


Count of samples between M-e and M+e. 


sampleCount 


Samples between median updates. 


n 


Current sample. 


X 


Input sample. 



1.2 Algorithm 

current o iz ADC value. X .8 the gyroscope voltage reading, which is an integer between 0 and 1023 

'' 8 nCren,en ' *** 1,mCmmt - *■»■*« °» «" **. °f X with 

2. Increment n 

3 * upS^ 0Unt (sampIeCount is °»™«y ™> which corresponds to approximately 1 second per 
" ^emenfeS 1." ^ " if ^^dowCount+equalCount). Also 

b ' mc^ein^ ^byl. " ** iS ^ M " 8 ' if ^wCoun^ChighCounHequalCount). Also 

°' Z t^nZ^ 0118 * ^ ^ " DOt *" ^ 016 medi -™*** -changed and e 




2 Gyroscope Calibration Program 

The aim is to find a value of Zg and k so that 
Where: 



(2-1) 



(a is the angular velocity, 
k is the gain. . 

v g is the Gyroscope ADC Value. 

zg is the Gyroscope ADC Value corresponding to zero °/s 

values fern, .teLuor are saSJZ ^ f ° f period - ^ ^ 

<a*en plae. toe ^ M <^^&^ 5 ^^^^ * 

M«*ure fl,e star, and finish a^ta using fc ado vaju^and fte cautaauon value* already stored in ft. sensor 



We know that 



Ae = 0 / -G / =itf(v,-z> 



A9 = 



Do n measurements. For the ith measurement: 
Several measurements are taken for Xi , y,, and T,. A least 



(2-2) 



(2-3) 



(2-4) 



squares best fit is used to calculate k and Zg. 



[-'..] 



( 2-5) can be rewritten: 



(2-5) 




y=Ab 

This equation is solved in a least squares sense using: C ^ 

A T y = A T Ab 
b = (A T A)A T y 

so that: < 2_7 ) 

» 

The values ofkand^areconverted to caUbratio^ 
The angular velocity in the Sensor Pack is calculated using the formula: 

(2-10) 

So K,, can be calculated using AT. = 8192 

* 8°/^.(_Ar) 



(2-8) 



(2-9) 
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